
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <cstring>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <jsoncpp/json/json.h>
#include <sstream>

#include <arpa/inet.h>
#include "unitree_api/msg/request.hpp"
#include "common/ros2_sport_client.h"


#define BUFLEN 4096


class UdpReceiverNode : public rclcpp::Node {
public:
    UdpReceiverNode() : Node("udp_receiver"), running_(true) {
        this->declare_parameter<std::string>("host", "192.168.3.76");
        this->declare_parameter<int>("port", 5006);
        std::string host = this->get_parameter("host").as_string();
        int port = this->get_parameter("port").as_int();

        // 创建运动控制发布器
        req_puber_ = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 10);


        int sockfd;
        struct sockaddr_in servaddr;

        if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
            RCLCPP_ERROR(this->get_logger(), "socket creation failed");
            throw std::runtime_error("socket creation failed");
        }

        memset(&servaddr, 0, sizeof(servaddr));
        servaddr.sin_family = AF_INET;
        servaddr.sin_addr.s_addr = inet_addr(host.c_str());
        servaddr.sin_port = htons(port);

        if (bind(sockfd, (const struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) {
            RCLCPP_ERROR(this->get_logger(), "bind failed");
            close(sockfd);
            throw std::runtime_error("bind failed");
        }

        RCLCPP_INFO(this->get_logger(), "[UDP] 监听 %s:%d ...", host.c_str(), port);

        // 启动接收线程
        receiver_thread_ = std::thread([this, sockfd]() {
            SportClient sport_client;
            bool stop_sent = false;
            struct sockaddr_in cliaddr;
            char buffer[BUFLEN];
            bool angle_computed = false;
            double target_angle = 0.0;
            double target_angle_deg = 0.0;
            int sign = 0;
            double angle_threshold = 0.05; // 允许误差
            double fixed_vyaw = 0.3; // 固定角速度
            static double turned_angle = 0.0;
            static int last_sign = 0;
            while (rclcpp::ok() && running_) {
                socklen_t len = sizeof(cliaddr);
                ssize_t n = recvfrom(sockfd, buffer, BUFLEN - 1, 0, (struct sockaddr *)&cliaddr, &len);
                if (n < 0) {
                    RCLCPP_WARN(this->get_logger(), "recvfrom error");
                    continue;
                }
                buffer[n] = '\0';
                std::string msg_str(buffer);
                try {
                    Json::Value root;
                    Json::CharReaderBuilder builder;
                    std::string errs;
                    std::istringstream s(msg_str);
                    if (!Json::parseFromStream(builder, s, &root, &errs)) {
                        RCLCPP_WARN(this->get_logger(), "[UDP] JSON解析失败: %s", errs.c_str());
                        continue;
                    }
                    // 只在首次收到有效position时计算和控制
                    if (!angle_computed && root.isMember("position")) {
                        double x = 0.0, y = 0.0;
                        const auto& pos = root["position"];
                        if (pos.isObject()) {
                            x = pos.get("x", 0.0).asDouble();
                            y = pos.get("y", 0.0).asDouble();
                        } else if (pos.isArray() && pos.size() >= 2) {
                            x = pos[0].asDouble();
                            y = pos[1].asDouble();
                        } else {
                            RCLCPP_WARN(this->get_logger(), "[UDP] position字段格式错误");
                            continue;
                        }
                        target_angle = std::atan2(y, x);
                        target_angle_deg = target_angle * 180.0 / M_PI;
                        sign = (target_angle > 0) ? 1 : ((target_angle < 0) ? -1 : 0);
                        RCLCPP_INFO(this->get_logger(), "[UDP] 目标角度: %.2f° (%.3f rad)", target_angle_deg, target_angle);
                        angle_computed = true;
                    }
                    if (angle_computed && !stop_sent) {
                        if (std::abs(target_angle) > angle_threshold) {
                            unitree_api::msg::Request req;
                            double vyaw = sign * fixed_vyaw;
                            sport_client.Move(req, 0.0, 0.0, vyaw);
                            req_puber_->publish(req);
                            // 累计已转角度（假设每次调用间隔0.1s）
                            double dt = 0.1; // 估算周期
                            turned_angle += vyaw * dt;
                            RCLCPP_INFO(this->get_logger(), "[UDP] 目标角:%.3f, 已转:%.3f, vyaw=%.3f (Move)", target_angle, turned_angle, vyaw);
                            // 判断是否到达目标角度
                            if ((last_sign != 0 && sign != last_sign) || std::abs(turned_angle) >= std::abs(target_angle)) {
                                // 到达或越过目标角度，发送停止
                                sport_client.StopMove(req);
                                req_puber_->publish(req);
                                RCLCPP_INFO(this->get_logger(), "[UDP] 已到达目标角度，发送StopMove，节点即将退出");
                                stop_sent = true;
                                running_ = false;
                            }
                            last_sign = sign;
                        } else {
                            unitree_api::msg::Request req;
                            sport_client.StopMove(req);
                            req_puber_->publish(req);
                            RCLCPP_INFO(this->get_logger(), "[UDP] 角度%.3f已在阈值内，已发送StopMove，节点即将退出", target_angle);
                            stop_sent = true;
                            running_ = false;
                        }
                    } else {
                        RCLCPP_WARN(this->get_logger(), "[UDP] JSON中无position字段");
                    }
                } catch (const std::exception& e) {
                    RCLCPP_WARN(this->get_logger(), "[UDP] 解析异常: %s", e.what());
                }
            }
            close(sockfd);
        });
        receiver_thread_.detach();
    }
private:
    std::thread receiver_thread_;
    std::atomic<bool> running_;
    rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr req_puber_;
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<UdpReceiverNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
